import sys, os, glob, struct
import socket
import time

DATAFILE = r"data.txt"


def parse_file(filename):

    #head=[x for x in range(21)]
    frames=[]

    mocap_file = open(filename, 'rb')
    while True:
        lines = mocap_file.readlines(100000)
        if not lines:
            break

        parsed_frame_count = len(frames)
        frames.extend([0]*len(lines))

        for index, line in enumerate(lines):
            ln = line.strip()

            rawdata = [float(x) for x in ln.split('\t')[1:]] # skip letter 'M'
            valid_data = rawdata[15:]
            pos = valid_data[0:3]
            qua=[]
            for i in range(18):
                idx_start = i*4+3
                idx_end = idx_start+4
                qua.append(i)
                qua.extend(valid_data[idx_start:idx_end])

            #frame = tuple(head+pos+qua)
            #fmt = struct.Struct('!21c3f')

            frame = tuple(pos+qua)

            fmt1 = struct.Struct('!21x3f')  # pack position
            fmt2 = struct.Struct('!i4f')    # pack quat(id, qw,qx,qy,qz)

            packed_frame=fmt1.pack(*frame[0:3])
            for i in range(18):
                idx_start=i*5+3
                idx_end = i*5+5+3
                packed_frame += fmt2.pack(*frame[idx_start:idx_end])

            #print packed_frame
            frames[parsed_frame_count+index] = packed_frame
            print len(packed_frame)

    # send via udp endless
    print len(frames)
    address = ('127.0.0.1', 8877)
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    while True:
        for frm in frames:
            sock.sendto(frm, address)
            time.sleep(1.0/60.0)

    mocap_file.close()

parse_file(DATAFILE)
